
    %% 参数设置
    N = 10;              % 发射可移动天线数
    K = 1;               % 接收用户数
    P = 20;              % 粒子数量
    Q = 100;             % 最大迭代次数
    d = 0.1;             % 天线之间的间距(m)
    M = 11     ;         % 每个子ULA的天线数
    Gamma = M ;              % 最小子阵列中心之间的间距(m)的统计单位
    d_min = (Gamma - 1)*d;    % 最小子阵列中心之间的间距(m)                   
    S = (M - 1)*d;       % 每个子ULA的整体孔径 1(m)  
    D = 12;               % mXL-ULA的整体孔径 (m)
    lambda = 0.01;             % 假设波长10mm，频率30GHz
    % $\max\{5D, \frac{4SD}{\lambda}\} \leq r < \frac{2D^2}{\lambda}$
    % $\max\{5D, \frac{4SD}{\lambda}\} < \frac{2D^2}{\lambda}$  
    % S < D/2,  \lambda < 0.4D
    % $\max\{5D, \frac{4SD}{\lambda}\} \leq \frac{2D_\mathrm{mov}^2}{\lambda} \leq r < \frac{2D^2}{\lambda}$
    

    omega_max = 0.9;     % 初始惯性权重
    omega_min = 0.4;     % 最小惯性权重
    c1 = 2;              % 个体学习因子
    c2 = 2;              % 全局学习因子
    p_max = 10;          % 最大功率约束
    p_th = 1;            % 最大干扰功率约束
    dR_max = 0.5;        % 初始邻域半径

    %% 初始化粒子的位置和速度
    dim = 2 * (N + K); % 位置维度
    U = rand(dim, P);  % 粒子位置  U的维度是   2*(N+K)  P
    V = rand(dim, P);  % 粒子速度
    Ct_bounds = [0, 0; -6, 6];
    Cr_bounds = [6, 6; -2, 2];
    % 确保初始粒子满足约束条件 C2 和 C3
    for p = 1:P
        U(:, p) = enforce_constraints(U(:, p), d_min, Ct_bounds, Cr_bounds);
    end
    % 从位置到信道
    H = position2userchannel(P, U, M, lambda, d); % 信道H的维度是N*M \times P
    H_w = position2williechannel(P, U, M, lambda, d);

    % 先解w，然后初始化fitness函数。   
    % 这一步每一个粒子（位置）都对应一个w的解，
    % 所以每一个粒子都需要求一个对应的w以及对应的fitness函数
    W_opt = zeros(N*M , P);
    for p = 1:P
        W_opt(:,p) = solve_SDP(H(:,p), H_w(:,p), p_max, p_th);
    end

    % 算出每个粒子对应的fitness函数
    Fit = zeros(P , 1);
    for p = 1:P
        Fit(p) = fitness_each(U(:, p), W_opt(:,p), H(:,p), d_min);
    end
    Fitbestp = zeros(P , 1);
    Fitbestp = Fit;    % 记录对于每个粒子的最优适应值

    % 从初始化粒子群中选出根据fitness函数选出个人最优和全局最优
    % 记录对于每个粒子的最优适应值对应的位置
    U_pbest = U;    % 维度仍然为 2*(N+K) P   这里按照整体更新

    % fitness_pbest = arrayfun(@(p) fitness_each(U(:, p), W_opt(:,p), H(:,p), d_min) , 1:P);
    [global_best_fitness, gbest_idx] = max(Fit);
    u_gbest = U(:, gbest_idx);
    % [a,b] = max([9,10,11,12,13])

    %% 主循环
    for q = 1:Q   % 粒子群移动次数 

        % 更新剪枝删减后的粒子数量
        P = size(U,2)
        % 更新惯性权重
        omega = omega_max - (omega_max - omega_min) * q / Q;   % 见论文page14

        % 更新速度和位置 
        for p = 1:P
            % 更新速度和位置  这里对应(87)和(89)中的更新
            r1 = rand(dim, 1);
            r2 = rand(dim, 1);
            V(:, p) = omega * V(:, p) ...
                      + c1 * r1 .* (U_pbest(:, p) - U(:, p)) ...
                      + c2 * r2 .* (u_gbest - U(:, p));
            U(:, p) = U(:, p) + V(:, p);

            % 投影到可行域
            U(:, p) = enforce_constraints(U(:, p), d_min, Ct_bounds, Cr_bounds);
     
            % 更新适应度值  这里对应(79)和(78)
            H(:,p) = perparticle_position2userchannel(U(:, p), M, lambda, d); % 信道H的维度是N*M \times P
            H_w(:,p) = perparticle_position2williechannel(U(:, p), M, lambda, d);
            W_opt(:,p) = solve_SDP(H(:,p), H_w(:,p), p_max, p_th);
            Fit(p) = fitness_each(U(:, p), W_opt(:,p), H(:,p), d_min);
            % 更新个人最优
            if Fit(p) > Fitbestp(p)
                Fitbestp(p) = Fit(p);
                U_pbest(:, p) = U(:, p);
            end
            % 更新全局最优
            if Fit(p) > global_best_fitness
                global_best_fitness = Fit(p);
                u_gbest = U(:, p);
            end
        end
        % 此时对粒子群中的所有粒子均更新过一轮
        % 粒子群整体U并没有变化，只是通过筛选算出了一些最好的和最差的和对应位置

        % 动态邻域剪枝
        dR = dR_max * (1 - q / Q); % 动态半径
        for p = 1:P
            if norm(U(:, p) - u_gbest) < dR
                % 如果粒子在邻域内，随机重新初始化
                U(:, p) = [];  % 对整个群体的所有点重新初始化？
                % U(:, p) = enforce_constraints(U(:, p), d_min, Ct_bounds, Cr_bounds);
            end
        end

        % 显示当前迭代的全局最优值
        fprintf('Iteration %d, Best Fitness: %.4f\n', q, global_best_fitness);
    end

    % 输出结果
    disp('Optimal solution:');
    disp(u_gbest);



